/*
 * PopTree.cpp
 *
 *  Created on: May 28, 2012
 *      Author: pickrell
 */



#include "PopTree.h"
using namespace std;

PopTree::PopTree(){
	enum {A, B, C, D, E, N};
	const int num_vertices = N;
	const char * name = "ABCDE";
	Edge edge_array[] =
	{ Edge(A,B), Edge(A,D), Edge(C,A), Edge(D,C),
	      Edge(C,E), Edge(B,D), Edge(D,E) };
	const int num_edges = sizeof(edge_array)/sizeof(edge_array[0]);
	g = Graph(num_vertices);
	//for (int i = 0; i < num_edges; ++i) add_edge(edge_array[i].first, edge_array[i].second, g);
}


PopTree::PopTree(vector<string> first3pops, vector<vector<bool> > ind, int l, int nm, vector<int> c, PhyloPop_params * pa){
	phi = (1+sqrt(5))/2;
	resphi = 2-phi;
	params = pa;
	len = l;
	nsite = ind[0].size();
	nmono = nm;
	colcounts = c;

	g = Graph(1);
	Graph::vertex_descriptor v = *vertices(g).first;
	Graph::vertex_descriptor v2, v3, v4, v5, v6;
	Graph::edge_descriptor e;
	g[v].index = 0;
	g[v].name = "NA";
	g[v].height = 0.02;
	g[v].is_tip = false;
	g[v].is_root = true;
	g[v].rev = false;
	for (int i = 0; i < nsite; i++){
		g[v].l0.push_back(-1000);
		g[v].l1.push_back(-1000);
	}
	g[v].lm = 0;
	g[v].pop =0;
	root = v;

	// add the first three populations to the tree
	// tree will look like this:
	//
	//           root
	//            /\
	//           /  \
	//          /   /\
	//         1    2 3
	if (first3pops.size() != 3) {
		std::cout << "Error initializing PopTree: need 3 pops, not "<< first3pops.size() << "\n";
		exit(1);
	}
	v3 = add_vertex(g);
	g[v3].index = 1;
	g[v3].name = first3pops[0];

	g[v3].height = 0;
	g[v3].is_tip = true;
	g[v3].is_root = false;
	g[v3].rev = false;
	g[v3].pop = 0;
	g[v3].lm = 0;
	vector<bool> d1 = ind[0];
	for (int i  = 0; i < d1.size(); i++){
		if (d1[i]){
			g[v3].l0.push_back(-1000);
			g[v3].l1.push_back(0);
		}
		else{
			g[v3].l0.push_back(0);
			g[v3].l1.push_back(-1000);
		}
	}

	e = add_edge( v, v3, g).first;
	g[e].weight = 1;
	g[e].len = 0.2;



	v4 = add_vertex(g);
	g[v4].index = 2;
	g[v4].name = "NA";
	g[v4].height = 0.01;
	g[v4].is_tip = false;
	g[v4].is_root = false;
	g[v4].rev = false;
	g[v4].pop = 0;
	g[v4].lm = 0;
	for (int i = 0; i < nsite; i++){
		g[v4].l0.push_back(-1000);
		g[v4].l1.push_back(-1000);
	}
	e = add_edge( v, v4, g).first;
	g[e].weight = 1;
	g[e].len = 0.1;



	v5 = add_vertex(g);
	g[v5].index = 3;
	g[v5].name = first3pops[1];
	g[v5].height = 0;
	g[v5].is_tip = true;
	g[v5].is_root = false;
	g[v5].rev = false;
	g[v5].pop = 0;
	g[v5].lm = 0;
	vector<bool> d2 = ind[1];
	for (int i  = 0; i < d2.size(); i++){
		if (d2[i]){
			g[v5].l0.push_back(-1000);
			g[v5].l1.push_back(0);
		}
		else{
			g[v5].l0.push_back(0);
			g[v5].l1.push_back(-1000);
		}
	}

	e = add_edge( v4, v5, g).first;
	g[e].weight = 1;
	g[e].len = 0.1;


	v6 = add_vertex(g);
	g[v6].index = 4;
	g[v6].name = first3pops[2];
	g[v6].height = 0;
	g[v6].is_tip = true;
	g[v6].is_root = false;
	g[v6].lm = 0;
	vector<bool> d3 = ind[2];
	for (int i  = 0; i < d3.size(); i++){
		if (d3[i]){
			g[v6].l0.push_back(-1000);
			g[v6].l1.push_back(0);
		}
		else{
			g[v6].l0.push_back(0);
			g[v6].l1.push_back(-1000);
		}
	}

	g[v6].rev = false;
	g[v6].pop = 0;
	e = add_edge( v4, v6, g).first;
	g[e].weight = 1;
	g[e].len = 0.1;
	indexcounter = 5;
	ntip = 3;
}



void PopTree::set_UPGMA(Alignment * aln){
	//cout << "setting UPGAMA\n"; cout.flush();
	g.clear();
	indexcounter = 0;
	vector<vector<double> > initdist = aln->get_dists();
	vector<string> hapnames = aln->list_haps();
	vector<Graph::vertex_descriptor> still;

	//cout << "got dists\n"; cout.flush();
	for (int i = 0; i < hapnames.size(); i++){
		Graph::vertex_descriptor added = add_vertex(g);
		g[added].index = indexcounter;
		g[added].name = hapnames[i];
		g[added].height = 0;
		g[added].is_tip = true;
		g[added].is_root = false;
		g[added].rev = false;
		vector<bool> h = aln->get_hap(hapnames[i]);
		for (int i = 0; i < nsite; i++){
			if (h[i]){
				g[added].l0.push_back(-1000);
				g[added].l1.push_back(0);
			}
			else{
				g[added].l0.push_back(0);
				g[added].l1.push_back(-1000);
			}
		}
		g[added].lm = 0;
		g[added].pop =0;
		indexcounter++;
		still.push_back(added);
	}
	/*
	for (int i = 0; i < initdist.size(); i++){
		cout << get_newick_format(still[i]) << " ";
	}

	cout << "\n";
	for (int i = 0; i < initdist.size(); i++){
		cout << get_newick_format(still[i]) << " ";
		for (int j = 0; j < initdist.size(); j++){
			cout << initdist[i][j] << " ";
		}
		cout << "\n";
	}
	cout <<"\n";
	*/

	//cout << "added tips\n"; cout.flush();
	while (still.size() > 1){
		//cout << "start still size: "<< still.size()<< "\n"; cout.flush();
		pair<pair<int, int>, double> tomerge = get_mindist( &still, &initdist);
		int index1 = tomerge.first.first;
		int index2 = tomerge.first.second;
		double newd = tomerge.second/2.0;

		//add the parent node
		Graph::vertex_descriptor added = add_vertex(g);
		g[added].index = indexcounter;
		g[added].name = "NA";
		g[added].height = newd;
		g[added].is_tip = false;
		g[added].is_root = false;
		if (still.size()==2) {
			g[added].is_root = true;
			root = added;
		}
		g[added].rev = false;
		for (int i = 0; i < nsite; i++){
				g[added].l0.push_back(-1000);
				g[added].l1.push_back(-1000);
		}
		g[added].lm = 0;
		g[added].pop =0;
		indexcounter++;

		//connect to children
		Graph::vertex_descriptor c1 = still[index1];
		Graph::vertex_descriptor c2 = still[index2];
		Graph::edge_descriptor e = add_edge(added, c1, g).first;
		g[e].weight = 1;
		g[e].len = g[added].height - g[c1].height;
		e = add_edge(added, c2, g).first;
		g[e].weight = 1;
		g[e].len = g[added].height - g[c2].height;

		//cout <<"added\n"; cout.flush();
		//new distances
		double n1 = (double) get_tips(c1).size();
		double n2 = (double) get_tips(c2).size();
		vector<double> newdists;
		for (int i = 0; i < still.size(); i++){
			if (i == index1 or i == index2 ) continue;
			else{
				double toadd = n1 / (n1+ n2) * initdist[i][index1];
				toadd+= n2/ (n1+n2) * initdist[i][index2];
				newdists.push_back(toadd);
				initdist[i].push_back(toadd);
			}
			//cout << "going to delete "<< index1 << " "<< index2 <<"\n"; cout.flush();
			//for (vector<vector<double> >::iterator it = initdist.begin(); it != initdist.end(); it++){
			//	for (vector<double>::iterator it2 = it->begin(); it2 != it->end(); it2++){
			///		cout << *it2 << " ";
			//	}
			//	cout << "\n";

			initdist[i].erase(initdist[i].begin()+ index1);
			initdist[i].erase(initdist[i].begin()+(index2 -1));
			//cout << "ok\n"; cout.flush();
		}
		newdists.push_back(0.0);
		initdist.push_back(newdists);
		still.push_back(added);
		//cout << "deleting\n"; cout.flush();

		// delete
		initdist.erase(initdist.begin()+index1);
		initdist.erase(initdist.begin()+ (index2-1));
		still.erase(still.begin()+index1);
		still.erase(still.begin()+ (index2-1));

/*
		for (int i = 0; i < initdist.size(); i++){
			cout << get_newick_format(still[i]) << " ";
		}
		cout << "\n";
		for (int i = 0; i < initdist.size(); i++){
			cout << get_newick_format(still[i]) << " ";
			for (int j = 0; j < initdist.size(); j++){
				cout << initdist[i][j] << " ";
			}
			cout << "\n";
		}
		cout <<"\n";
		*/
		//cout << "still size: "<< still.size()<< "\n"; cout.flush();
	}


}


double PopTree::set_random_UPGMA(Alignment * aln, double lambda, double N, double mu, gsl_rng * r){
	double toreturn = 0;
	//cout << "setting UPGAMA\n"; cout.flush();
	g.clear();
	indexcounter = 0;
	vector<vector<double> > alldist = aln->get_dists();
	vector<vector<double> > initdist = aln->get_dists();
	vector<string> hapnames = aln->list_haps();
	vector<Graph::vertex_descriptor> still;

	//cout << "got dists\n"; cout.flush();
	for (int i = 0; i < hapnames.size(); i++){
		Graph::vertex_descriptor added = add_vertex(g);
		g[added].index = indexcounter;
		g[added].name = hapnames[i];
		g[added].height = 0;
		g[added].is_tip = true;
		g[added].is_root = false;
		g[added].rev = false;
		vector<bool> h = aln->get_hap(hapnames[i]);
		for (int i = 0; i < nsite; i++){
			if (h[i]){
				g[added].l0.push_back(-1000);
				g[added].l1.push_back(0);
			}
			else{
				g[added].l0.push_back(0);
				g[added].l1.push_back(-1000);
			}
		}
		g[added].lm = 0;
		g[added].pop =0;
		indexcounter++;
		still.push_back(added);
	}

	/*
	for (int i = 0; i < initdist.size(); i++){
		cout << get_newick_format(still[i]) << " ";
	}

	cout << "\n";
	for (int i = 0; i < initdist.size(); i++){
		cout << get_newick_format(still[i]) << " ";
		for (int j = 0; j < initdist.size(); j++){
			cout << initdist[i][j] << " ";
		}
		cout << "\n";
	}
	cout <<"\n";
*/

	//cout << "added tips\n"; cout.flush();
	while (still.size() > 1){
		//cout << "start still size: "<< still.size()<< "\n"; cout.flush();
		pair<pair<int, int>, double> tomerge = get_randdist( &still, &initdist, lambda, r, &toreturn);
		int index1 = tomerge.first.first;
		int index2 = tomerge.first.second;
		double newd = tomerge.second/2.0;
		Graph::vertex_descriptor c1 = still[index1];
		Graph::vertex_descriptor c2 = still[index2];
		//cout << "chose " << get_newick_format(c1) << " "<< get_newick_format(c2) << "\n";

		double h1 = g[c1].height;
		double h2 = g[c2].height;
		double maxd = h1;
		double otherd = h2;
		if (h2 > h1) {
			maxd = h2;
			otherd = h1;
		}

		if (newd < maxd) newd = maxd;
		map<string, Graph::vertex_descriptor> tip1 = get_tips(c1);
		map<string, Graph::vertex_descriptor> tip2 = get_tips(c2);
		double n1 = (double) tip1.size();
		double n2 = (double) tip2.size();

		int current_size = still.size();
		int perm = current_size * (current_size - 1) / 2;
		double expl = (double) perm / (double)N;
		double alpha = expl;
		//double alpha = expl;
		//cout << "before "<< alpha << "\n";
		alpha = 1/alpha * mu * (double) aln->len;
		//cout << "prior w/ "<< current_size << " "<< alpha << "\n";
		//cout << perm << " "<< N << " "<< mu << " " << expl << " "<< (double) aln->len << "\n";
		double postd = get_postd(newd, maxd, (int) n1+n2, alpha, (double) aln->len,  r, &toreturn);
		//add the parent node
		Graph::vertex_descriptor added = add_vertex(g);
		g[added].index = indexcounter;
		g[added].name = "NA";
		g[added].height = maxd+postd;
		g[added].is_tip = false;
		g[added].is_root = false;
		if (still.size()==2) {
			g[added].is_root = true;
			root = added;
		}
		g[added].rev = false;
		for (int i = 0; i < nsite; i++){
				g[added].l0.push_back(-1000);
				g[added].l1.push_back(-1000);
		}
		g[added].lm = 0;
		g[added].pop =0;
		indexcounter++;

		//connect to children

		Graph::edge_descriptor e = add_edge(added, c1, g).first;
		g[e].weight = 1;
		g[e].len = g[added].height - g[c1].height;
		e = add_edge(added, c2, g).first;
		g[e].weight = 1;
		g[e].len = g[added].height - g[c2].height;

		//cout <<"added\n"; cout.flush();

		// new distances
		vector<double> newdists;
		for (int i = 0; i < still.size(); i++){
			if (i == index1 or i == index2 ) continue;
			else{
				//cout << n1 << " "<< n2 << " "<< initdist[i][index1] <<  " toadd\n";
				double toadd = n1 / (n1+ n2) * initdist[i][index1];
				toadd+= n2/ (n1+n2) * initdist[i][index2];
				newdists.push_back(toadd);

				initdist[i].push_back(toadd);
			}
			//cout << "going to delete "<< index1 << " "<< index2 <<"\n"; cout.flush();
			//for (vector<vector<double> >::iterator it = initdist.begin(); it != initdist.end(); it++){
			//	for (vector<double>::iterator it2 = it->begin(); it2 != it->end(); it2++){
			///		cout << *it2 << " ";
			//	}
			//	cout << "\n";

			initdist[i].erase(initdist[i].begin()+ index1);
			initdist[i].erase(initdist[i].begin()+(index2 -1));
			//cout << "ok\n"; cout.flush();
		}
		newdists.push_back(0.0);
		initdist.push_back(newdists);
		still.push_back(added);
		//cout << "deleting\n"; cout.flush();

		// delete
		initdist.erase(initdist.begin()+index1);
		initdist.erase(initdist.begin()+ (index2-1));
		still.erase(still.begin()+index1);
		still.erase(still.begin()+ (index2-1));

/*
		for (int i = 0; i < initdist.size(); i++){
			cout << get_newick_format(still[i]) << " ";
		}
		cout << "\n";
		for (int i = 0; i < initdist.size(); i++){
			cout << get_newick_format(still[i]) << " ";
			for (int j = 0; j < initdist.size(); j++){
				cout << initdist[i][j] << " ";
			}
			cout << "\n";
		}
		cout <<"\n";
*/
		//cout << "still size: "<< still.size()<< "\n"; cout.flush();
	}

	return toreturn;
}

double PopTree::get_postd(double newd, double maxd, int nt, double alpha, double len, gsl_rng *r , double * toup){
	double toreturn;
	double s = (newd-maxd)* len;
	double newalpha = alpha+ s*nt;
	double newbeta = 1 + nt;
	toreturn = gsl_ran_gamma (r, newalpha, 1/newbeta);
	double toadd = get_loggamma(toreturn, newalpha, 1/newbeta);
	if (toadd < log(DBL_MIN)) toadd = log(DBL_MIN);
	if (toadd > log(DBL_MAX)) toadd = log(DBL_MAX);
	cout << toreturn << " "<< newalpha << " "<< 1/newbeta << " "<< toadd << " "<< *toup << "  in postd\n";
	(*toup) += toadd;
	toreturn = toreturn/len;

	//scout << newalpha << " "<< newbeta << " "<< toreturn << "\n";
	return toreturn;
}

double PopTree::get_loggamma(double x, double a, double b){
//{1 \over \Gamma(a) b^a} x^{a-1} e^{-x/b}
	double toreturn = 0;
	toreturn += -gsl_sf_lngamma (a) - a*log(b);
	toreturn += (a-1)*log(x);
	toreturn += -x/b;
	return toreturn;
}

pair< pair<int, int>, double> PopTree::get_mindist(vector<Graph::vertex_descriptor>* s, vector<vector<double> >* dists){
	pair<pair<int, int>, double> toreturn;
	int min1, min2;
	double mindist = 10000000;
	for (int i = 0; i < s->size(); i++ ){
		for (int j = i+1; j < s->size(); j++){
			double d = dists->at(i).at(j);
			if (d < mindist){
				mindist = d;
				min1 = i;
				min2 = j;
			}
		}
	}
	if (min1 < min2){
		toreturn.first.first = min1;
		toreturn.first.second = min2;
	}
	else{
		toreturn.first.first = min2;
		toreturn.first.second = min1;
	}
	toreturn.second = mindist;
	return toreturn;
}



pair< pair<int, int>, double> PopTree::get_randdist(vector<Graph::vertex_descriptor>* s, vector<vector<double> >* dists, double lambda, gsl_rng *r , double* toup){
	pair<pair<int, int>, double> toreturn;
	int min1, min2;
	vector<pair<int, int> > indices;
	vector<double> weights;
	vector<double> di;
	double mindist = 10000000;
	double sumweight = 0;
	for (int i = 0; i < s->size(); i++ ){
		for (int j = i+1; j < s->size(); j++){
			double d = dists->at(i).at(j);
			if (d < mindist) mindist = d;
			weights.push_back(d);
			di.push_back(d);
			indices.push_back(make_pair(i, j));
		}
	}
	for (int i = 0; i < weights.size(); i++) {
		double tmp = 1-(weights[i]- mindist)* lambda;
		//cout << tmp << " "<< weights[i] << " "<< mindist << " "<< lambda << "\n";
		if (tmp < 0) tmp = 0;
		weights[i] = tmp;
		sumweight+= tmp;
	}
	double srand = gsl_rng_uniform(r)*sumweight;
	int i = 0;
	while (weights[i] < srand) {
		//cout << weights[i] << " "<< srand << " s\n";
		srand -= weights[i];
		i++;
	}
	double toadd = log(weights[i]) - log(sumweight);
	cout << weights[i]<< " "<< sumweight << " "<<  toadd <<" in randdist\n";
	(*toup)+= toadd;
	pair<int, int> selected = indices[i];
	min1 = selected.first;
	min2 = selected.second;

	if (min1 < min2){
		toreturn.first.first = min1;
		toreturn.first.second = min2;
	}
	else{
		toreturn.first.first = min2;
		toreturn.first.second = min1;
	}
	toreturn.second = di[i];
	return toreturn;
}


void PopTree::set_graph(string p_newickString){
	g.clear();
	add_vertex(g);
	index2father.clear();
	popnames.clear();
	ntip = 0;
	int i = 0;
	Graph::vertex_descriptor v = *vertices(g).first;
	Graph::vertex_descriptor v2 = *vertices(g).first;
	Graph::edge_descriptor e;
	g[v].index = i;
	g[v].name = "NA";
	g[v].height = 0;
	index2father.push_back(NULL);
	g[v].is_tip = false;
	g[v].is_root = true;
	g[v].rev = false;
	g[v].pop = 0;
	root = v;
	for(string::const_iterator I = p_newickString.begin();
		I != p_newickString.end(); ++I)
      {
		//cout << *I << "\n";
		if ( *I == '(' )//Begin new subtree
		{
			v2 = add_vertex(g);
			i++;
			g[v2].index = i;
			g[v2].name = "NA";
			g[v2].height = 0;
			index2father.push_back(v);
			g[v2].is_tip = false;
			g[v2].is_root = false;
			g[v2].rev = false;
			g[v2].pop = 0;
			e = add_edge( v, v2, g).first;
			v = v2;
		}
		else if( *I == ')' )// Subtree finished, get back
		{
			v = index2father[g[v].index];
		}
		else if( *I == ',' )// insert brother
		{
			v = index2father[g[v].index];
			v2 = add_vertex(g);
			i++;
			g[v2].index = i;
			g[v2].name = "NA";
			g[v2].height = 0;
			index2father.push_back(v);
			g[v2].is_tip = false;
			g[v2].is_root = false;
			g[v2].rev = false;
			g[v2].pop = 0;
			e = add_edge( v, v2, g).first;
			v = v2;
      }
      else if( *I == ':' )// treelength
      {
              std::string length = "";
              ++I;
              while( *I!=',' && *I!=')' && *I!=':' && *I!='(' && *I!=';')
              {
                      length += *I;
                      ++I;
              }
              --I;
              graph_traits<Graph>::in_edge_iterator in_i;
              graph_traits<Graph>::in_edge_iterator in_end;
              for ( tie(in_i, in_end)= in_edges(v, g); in_i != in_end; ++in_i ){
            	  g[*in_i].len = atof(length.c_str());
            	  g[*in_i].weight = 1;
              }
      }
      else if( *I == ';' )
      {
              break;
      }
      else// name
      {
              std::string name = "";
              do
              {
                      name += *I;
                      ++I;
              }
              while( *I!=',' && *I!=')' && *I!=':' && *I!='(' && *I!=';');
              --I;
              g[v2].name = name;
              g[v2].is_tip = true;
              popnames.push_back(name);
              ntip++;
      }
      }
	indexcounter = i+1;
	set_heights_from_edges();
}

void PopTree::set_heights_from_edges(){
	vector<Graph::vertex_descriptor> posto = get_postorder_traversal(ntip);
	for (vector<Graph::vertex_descriptor>::iterator it = posto.begin(); it != posto.end(); it++){
		if (g[*it].is_tip) continue;
		Graph::vertex_descriptor v = *it;
		Graph::out_edge_iterator outit = out_edges(v, g).first;
		Graph::edge_descriptor e = *outit;
		Graph::vertex_descriptor c = target(e, g);
		g[*it].height = g[c].height + g[e].len;
	}
}


pair<Graph::vertex_descriptor, Graph::vertex_descriptor> PopTree::get_child_nodes(Graph::vertex_descriptor v){
	Graph::vertex_descriptor c1 = v;
	Graph::vertex_descriptor c2 = v;
	if (out_degree(v, g) != 2){
		return make_pair(c1, c2);
	}
	graph_traits<Graph>::out_edge_iterator out_i = out_edges(v, g).first;
	c1 = target(*out_i, g);
	out_i++;
	c2 = target(*out_i, g);
	return make_pair(c1, c2);
}

map<string, Graph::vertex_descriptor> PopTree::get_tips(Graph::vertex_descriptor p_rootIterator){
	map<string, Graph::vertex_descriptor> toreturn;
  	if (out_degree(p_rootIterator, g) ==0){
 		toreturn.insert(make_pair(g[p_rootIterator].name, p_rootIterator));
 	}
 	else{
 		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children =  get_child_nodes(p_rootIterator);
 		map<string, Graph::vertex_descriptor> t1 = get_tips(children.first);


 		map<string, Graph::vertex_descriptor> t2 = get_tips(children.second);
 		for (map<string, Graph::vertex_descriptor >::iterator it1 = t1.begin(); it1 != t1.end(); it1++) toreturn.insert(make_pair(it1->first, it1->second));
 		for (map<string, Graph::vertex_descriptor>::iterator it2 = t2.begin(); it2 != t2.end(); it2++)	toreturn.insert(make_pair(it2->first, it2->second));

 	}
 	return toreturn;
}


void PopTree::copy(PopTree * s){
	popnames = s->popnames;


	//cout << "copying graph\n"; cout.flush();
	g.clear();
	ntip = s->ntip;
	nsite = s->nsite;
	nmono = s->nmono;
	len = s->len;

	colcounts.clear();
	//cout << nsite << "\n";
	//cout << "in  \n"; cout.flush();
	for (int i = 0; i < nsite; i++) colcounts.push_back( s->colcounts.at(i));
	//cout << "here \n"; cout.flush();
	map<int, Graph::vertex_descriptor> tmpmap;
	Graph::vertex_descriptor vd;
	pair<Graph::vertex_iterator, Graph::vertex_iterator> v = vertices(s->g);
	pair<Graph::edge_iterator, Graph::edge_iterator> e = edges(s->g);
	for (Graph::vertex_iterator it = v.first; it != v.second; ++it){
		vd = add_vertex(g);
		g[vd].index = s->g[*it].index;
		g[vd].name = s->g[*it].name;
		g[vd].height = s->g[*it].height;
		g[vd].rev = s->g[*it].rev;
		g[vd].is_root = s->g[*it].is_root;
		g[vd].is_tip = s->g[*it].is_tip;
		g[vd].pop = s->g[*it].pop;
		for (int i = 0; i <nsite; i++){
			g[vd].l0.push_back( s->g[*it].l0[i]);
			g[vd].l1.push_back( s->g[*it].l1[i]);
		}
		g[vd].lm = s->g[*it].lm;
		tmpmap.insert(make_pair( g[vd].index, vd));
		if (s->g[*it].is_root == true) root = vd;
	}

	for (Graph::edge_iterator it = e.first; it != e.second; ++it){
		Graph::vertex_descriptor tmpsource = source(*it, s->g);
		Graph::vertex_descriptor tmptarget = target(*it, s->g);
		Graph::edge_descriptor ed = add_edge( tmpmap[ s->g[tmpsource].index ], tmpmap[ s->g[tmptarget].index ], g ).first;
		g[ed].weight = s->g[*it].weight;
		g[ed].len = s->g[*it].len;
	}
}



void PopTree::print(string stem){
	string outvfile = stem+".vertices.gz";
	string outefile = stem+".edges.gz";
	ogzstream outv(outvfile.c_str());
	ogzstream oute(outefile.c_str());
	IndexMap index = get(&Node::index, g);

	pair<vertex_iter, vertex_iter> vp;
	for (vp = vertices(g); vp.first != vp.second; ++vp.first){
		outv << index[*vp.first] <<  " "<< g[*vp.first].name << " ";

		if (g[*vp.first].is_root) outv << "ROOT ";
		else outv << "NOT_ROOT ";
		outv << "NOT_MIG ";
		if (g[*vp.first].is_tip) outv << "TIP ";
		else outv << "NOT_TIP ";


		outv << g[get_parent_node(*vp.first).first].index << " ";
		if (g[*vp.first].is_tip) outv << "NA NA NA NA ";
		else {
			pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(*vp.first);
			outv << g[ch.first].index << " "<< get_tips(ch.first).size() << " "<< g[ch.second].index << " "<< get_tips(ch.second).size()<< " ";
		}

		outv << get_newick_format(*vp.first) <<" ";
		outv << g[*vp.first].height <<  " "<< g[*vp.first].pop <<  "\n";
	}

    graph_traits<Graph>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei){
    	double l = g[source(*ei, g)].height - g[target(*ei, g)].height;
        oute << index[source(*ei, g)] << " "<< index[target(*ei, g)] << " "<< l << " 1 NOT_MIG\n";
    }
}

pair<Graph::vertex_descriptor, double> PopTree::get_parent_node(Graph::vertex_descriptor v){
	pair<Graph::vertex_descriptor, double> toreturn;
	if (g[v].is_root) {
		toreturn.first = v;
		toreturn.second = 0;
		return toreturn;
	}

	graph_traits<Graph>::in_edge_iterator in_i = in_edges(v, g).first;
	toreturn.first = source(*in_i, g);
	toreturn.second = g[*in_i].len;
	return toreturn;
}


string PopTree::get_newick_format(){
 	string toreturn = "";
 	newick_helper(root, &toreturn);
 	return toreturn;
 }


string PopTree::get_newick_format(Graph::vertex_descriptor v){
 	string toreturn = "";
 	newick_helper(v, &toreturn);
 	return toreturn;
 }




void PopTree::local_rearrange(Graph::vertex_descriptor v3, int i){
	/*
	 *
	 * go from    1
	 *          /  \*
	 *         /    3
	 *        /    / \
	 *       2    4   5
	 *
	 *  if i == 1, go to
	 *
	 *           1
	 *          / \
	 *         3   \
	 *        / \   \
	 *       2   4   5
	 *
	 *  if i == 2, go to
	 *
	 *           1
	 *          / \
	 *         3   \
	 *        / \   \
	 *       2   5   4
	 *
	 *
	 */
	if (!g[v3].is_root & !g[v3].is_tip){
		//cout << "here\n"; cout.flush();
		// set up descriptors and edge lengths
		Graph::in_edge_iterator tmp = in_edges(v3, g).first;
		Graph::edge_descriptor ed = *tmp;
		Graph::vertex_descriptor v1, v2, v4, v5;

		double h1, h4, h5, h2;
		v1 = source(ed, g);
		h1 = g[v1].height;
		Graph::out_edge_iterator out3 = out_edges(v3, g).first;
		v4 = target(*out3, g);
		h4 = g[v4].height;

		++out3;
		v5 = target(*out3, g);
		h5 = g[v5].height;
		Graph::out_edge_iterator out1 = out_edges(v1, g).first;
		if (target(*out1, g) == v3 ) {
			out1++;
			v2 = target(*out1, g);
			h2 = g[v2].height;
		}
		else {
			v2 = target(*out1, g);
			h2 = g[v2].height;
		}


		if (i == 1){
		// go to ((2,4),5)

			remove_edge(v1, v2, g);
			remove_edge(v3, v5, g);
			Graph::edge_descriptor e = add_edge(v3, v2, g).first;
			g[e].weight = 1;
			if (h2> h4) g[v3].height = (h1+h2)/2;
			else g[v3].height = (h1+h4)/2;
			e = add_edge(v1, v5, g).first;
			g[e].weight = 1;

		}
		else{
			//go to (4,(2,5))

			remove_edge(v1, v2, g);
			remove_edge(v3, v4, g);
			Graph::edge_descriptor e = add_edge(v3, v2, g).first;
			g[e].weight = 1;

			if (h2> h5) g[v3].height = (h1+h2)/2;
			else  g[v3].height = (h1+h5)/2;
			//cout << g[v3].height << " "<< h1 << " "<< h2 << "\n";
			e = add_edge(v1, v4, g).first;
			g[e].weight = 1;


		}
		vector<Graph::vertex_descriptor> opt = get_path_to_root_vec(v3);
		//compute_tree_llk();
		//cout << current_llk<< "\n";
		optimize_heights(opt);
	}
	//cout << "nothere\n\n";
}

void PopTree::newick_helper(Graph::vertex_descriptor node, string* s){

	if (out_degree(node, g) !=  0){

		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children = get_child_nodes(node);
		s->append("(");
		newick_helper(children.first, s);
		s->append(",");
		newick_helper(children.second, s);
		s->append(")");
		if (g[node].is_root == false){

			s->append(":");

			stringstream ss;
			ss<< get_parent_node(node).second;
			s->append(ss.str());
		}
		else s->append(";");
	}
	else{

		stringstream ss;
		ss << g[node].name;
		ss << ":";

		ss<< get_parent_node(node).second;

		s->append(ss.str());

	}
}


vector<Graph::vertex_descriptor > PopTree::get_inorder_traversal(int nodes){
	vector<Graph::vertex_descriptor> toreturn(2*nodes-1);
	int count = 0;
	inorder_traverse(root, &count, &toreturn);
	return toreturn;
}

void PopTree::inorder_traverse(Graph::vertex_descriptor rootIterator, int* i, vector<Graph::vertex_descriptor >* v){
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> child_nodes = get_child_nodes(rootIterator);
	//graph_traits<Graph>::out_edge_iterator out_i = out_edges(rootIterator, g).first;
	//cout << g[rootIterator].index << " "<< g[rootIterator].name << " "<< g[rootIterator].is_tip << " "<< g[rootIterator].rev << "\n";
	if (g[rootIterator].is_tip == false){
		//inorder_traverse(target(*out_i, g), i , v);
		inorder_traverse(child_nodes.first, i , v);
 	}

 	v->at(*i) = rootIterator;
 	*i = *i+1;

 	if (g[rootIterator].is_tip == false){
 		inorder_traverse(child_nodes.second, i, v);
 	}
}



vector<Graph::vertex_descriptor > PopTree::get_postorder_traversal(int nodes){
	vector<Graph::vertex_descriptor> toreturn(2*nodes-1);
	int count = 0;
	postorder_traverse(root, &count, &toreturn);
	return toreturn;
}


vector<Graph::vertex_descriptor > PopTree::get_postorder_traversal(Graph::vertex_descriptor v, int nodes){
	vector<Graph::vertex_descriptor> toreturn(2*nodes-1);
	int count = 0;
	postorder_traverse(v, &count, &toreturn);
	return toreturn;
}

void PopTree::postorder_traverse(Graph::vertex_descriptor rootIterator, int* i, vector<Graph::vertex_descriptor >* v){
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> child_nodes = get_child_nodes(rootIterator);
	//graph_traits<Graph>::out_edge_iterator out_i = out_edges(rootIterator, g).first;
	//cout << g[rootIterator].index << " "<< g[rootIterator].name << " "<< g[rootIterator].is_tip << " "<< g[rootIterator].rev << "\n";
	if (g[rootIterator].is_tip == false){
		//inorder_traverse(target(*out_i, g), i , v);
		postorder_traverse(child_nodes.first, i , v);
 		postorder_traverse(child_nodes.second, i, v);
 	}
 	v->at(*i) = rootIterator;
	*i = *i+1;
}

double PopTree::sumlog(double logx, double logy){
	if (logx > logy) return logx + log(1 + exp(logy-logx));
	else return logy + log(1 + exp(logx-logy));

}
void PopTree::set_ls(Graph::vertex_descriptor v){
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(v);
	Graph::vertex_descriptor c1 = ch.first;
	Graph::vertex_descriptor c2 = ch.second;
	double t1 = g[v].height - g[c1].height;
	double t2 = g[v].height - g[c2].height;
	for (int i = 0; i < nsite; i++){
		//doing this in log space
		double ll1_1 = lk(1, 1, t1) + g[c1].l1[i];
		double ll1_0 = lk(1, 0, t1) + g[c1].l0[i];
		double ll1_first = sumlog(ll1_1, ll1_0);

		ll1_1 = lk(1, 1, t2)+ g[c2].l1[i];
		ll1_0 = lk(1, 0, t2)+ g[c2].l0[i];
		double ll1_second = sumlog(ll1_1, ll1_0);
		//cout << t1 << " "<< t2 << " "<< lk(1, 1, t1)<< " "<< lk(1, 0, t1)<< " "<< g[c1].l1[i]<< " "<< g[c1].l0[i]<< " "<< g[c2].l1[i]<< " "<< g[c2].l0[i]<< " "<< ll1_1 << " "<< ll1_0 << " "<< ll1_first << " "<< ll1_second << "\n";
		double ll1 = ll1_first+ ll1_second;

		double ll2_1 = lk(0, 1, t1)+ g[c1].l1[i];
		double ll2_0 = lk(0, 0, t1)+ g[c1].l0[i];
		double ll2_first =sumlog(ll2_1, ll2_0);
		ll2_1 = lk(0, 1, t2)+ g[c2].l1[i];
		ll2_0 = lk(0, 0, t2)+g[c2].l0[i];
		double ll2_second = sumlog(ll2_1, ll2_0);
		double ll0 = ll2_first + ll2_second;

		//double ll1 = ( lk(1, 1, t1) * g[c1].l1[i] + lk(1, 0, t1)* g[c1].l0[i] )*  ( lk(1, 1, t2) * g[c2].l1[i] + lk(1, 0, t2)* g[c2].l0[i] );
		//double ll0 = ( lk(0, 1, t1) * g[c1].l1[i] + lk(0, 0, t1)* g[c1].l0[i] )*  ( lk(0, 1, t2) * g[c2].l1[i] + lk(0, 0, t2)* g[c2].l0[i] );
		g[v].l0[i] = ll0;
		g[v].l1[i] = ll1;
		//cout << g[v].index << " "<< i << " "<< ll0 << " "<< ll1 << " "<< g[v].l0[i] << " " << g[v].l1[i] << " "<< g[c1].l1[i] << " "<< g[c2].l1[i] << "\n";
	}
	g[v].lm = g[c1].lm + g[c2].lm + t1+t2;
	//cout << g[v].index << " "<< g[v].lm << " "<< g[c1].lm  << " "<< g[c2].lm << " "<< t1 << " "<< t2 << "\n\n";
}

double PopTree::lk(int from, int to, double t){
	double toreturn;
	if (from == to) toreturn =  -t;
	else toreturn= log(t)-t;
	//cout << toreturn << "\n";
	return toreturn;
}

void PopTree::compute_tree_llk(){
	//cout << "ntip "<<  ntip << "\n";
	vector<Graph::vertex_descriptor> post = get_postorder_traversal(ntip);
	for (vector<Graph::vertex_descriptor>::iterator it = post.begin(); it != post.end(); it++){
		if (g[*it].is_tip) continue;

		set_ls(*it);
		//cout << g[*it].index << " "<< g[*it].lm << " "<< g[*it].name << " "<< g[*it].is_tip << "\n";
	}
	double llk = 0;
	double tmplm = g[root].lm;
	//cout << tmplm << " "<< log(lk(0, 0, tmplm)) <<" "<< nmono<<  "\n";
	llk += lk(0, 0, tmplm) * nmono;
	//cout << "llk mono " << llk << "\n";
	for (int i = 0; i < nsite; i++){
		//cout << log(g[root].l0[i]+ g[root].l1[i]) <<  " "<< i << "\n";
		double ll_first = log(0.5)+g[root].l0[i];
		double ll_second = log(0.5)+g[root].l1[i];
		double tmpl =  sumlog(ll_first, ll_second);
		llk += tmpl *colcounts.at(i);
		//cout << i << " "<< tmpl << " "<< tmplm << " " << nmono << "\n";
		//llk += log(0.5*g[root].l0[i]+ 0.5*g[root].l1[i]);
	}
	current_llk = llk;
}

Graph::vertex_descriptor PopTree::add_tip(Graph::vertex_descriptor v, vector<bool> ind, string name){
	// add a tip attached above v, halfway to the parent node
	Graph::vertex_descriptor vtipnew, vintnew, vparent;
	double ph, ch;
	Graph::edge_descriptor e;
	ch = g[v].height;
	ntip++;

	if (g[v].is_root == false){
		// remove edge from parent to target
		graph_traits<Graph>::in_edge_iterator init = in_edges(v, g).first;
		vparent = source(*init, g);
		ph = g[vparent].height;
		remove_edge(*init, g);

		//add new vertices
		vtipnew = add_vertex(g);
		vintnew = add_vertex(g);

		g[vtipnew].index = indexcounter;
		g[vtipnew].name = name;
		g[vtipnew].lm = 0;
		g[vtipnew].height = 0;
		g[vtipnew].is_tip = true;
		g[vtipnew].is_root = false;
		g[vtipnew].rev = false;
		g[vtipnew].pop = 0;
		for (int i  = 0; i < ind.size(); i++){
			if (ind[i]){
				g[vtipnew].l0.push_back(-1000);
				g[vtipnew].l1.push_back(0);
			}
			else{
				g[vtipnew].l0.push_back(0);
				g[vtipnew].l1.push_back(-1000);
			}
		}
		indexcounter++;

		g[vintnew].index = indexcounter;
		g[vintnew].name = "NA";
		g[vintnew].height = (ph+ch)/2;
		g[vintnew].is_tip = false;
		g[vintnew].is_root = false;
		g[vintnew].rev = false;
		g[vintnew].lm = 0;
		g[vintnew].pop = 0;
		for (int i  = 0; i < ind.size(); i++){
			g[vintnew].l0.push_back(-1000);
			g[vintnew].l1.push_back(-1000);
		}
		indexcounter++;

		// add new edges
		e = add_edge( vparent, vintnew, g).first;
		g[e].weight = 1;
		g[e].len = 0.1;

		e = add_edge( vintnew, v, g).first;
		g[e].weight = 1;
		g[e].len = 0.1;

		e = add_edge( vintnew, vtipnew, g).first;
		g[e].weight = 1;
		g[e].len = 0.1;

		set_ls(vintnew);
		vector<Graph::vertex_descriptor> opt = get_path_to_root_vec(vparent);
		optimize_heights(opt);
		return vtipnew;
	}
	else{

		//add a new root
		g[v].is_root = false;
		vintnew = add_vertex(g);
		vtipnew = add_vertex(g);
		root = vintnew;
		g[vtipnew].index = indexcounter;
		g[vtipnew].name = name;
		g[vtipnew].height = 0;
		g[vtipnew].is_tip = true;
		g[vtipnew].is_root = false;
		g[vtipnew].rev = false;
		g[vtipnew].pop  = 0;
		g[vtipnew].lm = 0;
		for (int i  = 0; i < ind.size(); i++){
			if (ind[i]){
				g[vtipnew].l0.push_back(-1000);
				g[vtipnew].l1.push_back(0);
			}
			else{
				g[vtipnew].l0.push_back(0);
				g[vtipnew].l1.push_back(-1000);
			}
		}
		indexcounter++;

		g[vintnew].index = indexcounter;
		g[vintnew].name = "NA";
		g[vintnew].height = ch+0.1;
		g[vintnew].is_tip = false;
		g[vintnew].is_root = true;
		g[vintnew].rev = false;
		g[vtipnew].pop = 0;
		g[vintnew].lm = 0;
		for (int i  = 0; i < ind.size(); i++){
			g[vintnew].l0.push_back(-1000);
			g[vintnew].l1.push_back(-1000);
		}
		indexcounter++;

		// add new edges
		e = add_edge( vintnew, v, g).first;
		g[e].weight = 1;
		g[e].len = 1;
		e = add_edge( vintnew, vtipnew, g).first;
		g[e].weight = 1;
		g[e].len = 1;

		set_ls(vintnew);
		vector<Graph::vertex_descriptor> opt;
		opt.push_back(vintnew);
		optimize_heights(opt);
		return vtipnew;
	}
}

void PopTree::compute_tree_llk(Graph::vertex_descriptor v){
	while (!g[v].is_root){
		if (g[v].is_tip) {
			v = get_parent_node(v).first;
			continue;
		}
		set_ls(v);
		v = get_parent_node(v).first;
	}
	set_ls(root);
	double llk = 0;
	double tmplm = g[root].lm;
	//cout << tmplm << " "<< log(lk(0, 0, tmplm)) <<" "<< nmono<<  "\n";
	llk += lk(0, 0, tmplm) * nmono;
	//cout << "llk mono " << llk << "\n";
	for (int i = 0; i < nsite; i++){
		//cout << log(g[root].l0[i]+ g[root].l1[i]) <<  " "<< i << "\n";
		double ll_first = log(0.5)+g[root].l0[i];
		double ll_second = log(0.5)+g[root].l1[i];
		double tmpl =  sumlog(ll_first, ll_second);
		//cout << i << " "<< tmpl << " "<< tmplm << " " << nmono << "\n";
		llk += tmpl *colcounts.at(i);
		//llk += log(0.5*g[root].l0[i]+ 0.5*g[root].l1[i]);
	}
	current_llk = llk;

}

double PopTree::conditional_llk(Graph::vertex_descriptor v){
	double toreturn = 0;
	//set_ls(v);
	double tmplm = g[v].lm;
	toreturn +=  lk(0, 0, tmplm) * nmono;
	for (int i = 0; i < nsite; i++){
		double ll_first = log(0.5)+g[v].l0[i];
		double ll_second = log(0.5)+g[v].l1[i];
		double tmpl = sumlog(ll_first, ll_second);
		toreturn += tmpl *colcounts.at(i);
	}
	return toreturn;
}


int PopTree::golden_section_h(Graph::vertex_descriptor in, Graph::vertex_descriptor eval, double min, double guess, double max, double tau, int* nit){
	double x;

	//cout << "running section\n"; cout.flush();
	//cout << guess << "\n"; cout.flush();
	if ( (max - guess) > (guess - min)) x = guess + resphi *( max - guess);
	else x = guess - resphi *(guess-min);
	if (fabs(max-min) < tau * (fabs(guess)+fabs(max)) || *nit > params->maxit) {
		double new_logweight = (min+max)/2;
		g[in].height = new_logweight;
		set_ls(in);
		set_ls(eval);
		//current_llik = llik();
		return 0;
	}
	*nit = *nit+1;
	double w = x;
	g[in].height = w;
	set_ls(in);
	set_ls(eval);
	double f_x = -conditional_llk(eval);
	//cout << w << " "<< -llik() << "\n";
	w = guess;
	g[in].height = w;
	set_ls(in);
	set_ls(eval);
	//cout << "not here2\n"; cout.flush();
	double f_guess = -conditional_llk(eval);
	//cout << guess << " "<< -llik() << "\n";

	if (f_x < f_guess){
		if ( (max-guess) > (guess-min) )	return golden_section_h(in, eval, guess, x, max, tau, nit);
		else return golden_section_h(in, eval, min, x, guess, tau, nit);
	}
	else{
		if ( (max - guess) > (guess - min)  ) return golden_section_h(in, eval, min, guess, x, tau, nit);
		else return golden_section_h(in, eval, x, guess, max, tau, nit);
	}
}

void PopTree::optimize_height(Graph::vertex_descriptor v){
	if (!g[v].is_root & !g[v].is_tip){
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(v);
		double m = g[ch.first].height;
		double m2 =g[ch.second].height;
		if (m2 > m) m = m2;
		Graph::vertex_descriptor p = get_parent_node(v).first;
		double ma = g[p].height;
		double h0 = g[v].height;
		double h = g[p].height - m;
		double add = h/10;
		if (h+2*add>g[p].height) add = -add;
		double lk0 = conditional_llk(p);
		double h1 = g[v].height+add;
		g[v].height = h1;
		set_ls(v);
		set_ls(p);
		double lk1 = conditional_llk(p);
		double h2 = g[v].height + add;
		g[v].height = h2;
		set_ls(v);
		set_ls(p);
		double lk2 = conditional_llk(p);
		bool stop = false;
		while (!stop){
			double iterate = quad_opt_h(h0, h1, h2, lk0, lk1, lk2, g[p].height-params->b_min, m+params->b_min);
			g[v].height = iterate;

			set_ls(v);
			set_ls(p);
			double newlk = conditional_llk(p);
			cout << iterate << " "<< newlk << " "<< h0 << " "<< h1 << " "<< h2 << " "<< lk0 << " "<< lk1 << " "<< lk2 << "\n";
			if ( fabs(newlk - lk2) < params->e_lk  ||  fabs(iterate - h2) < params->e_step) stop = true;
			else{
				h0 = h1;
				h1 = h2;
				h2 = iterate;
				lk0 = lk1;
				lk1 = lk2;
				lk2 = newlk;
			}
		}
	}
	else if (g[v].is_root){
		cout << "here1\n";
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(v);
		double m = g[ch.first].height;
		double m2 =g[ch.second].height;
		if (m2 > m) m = m2;
		double h0 = g[v].height;
		//double add = (h0-m)/10;
		double add = 0.01;
		cout << add << "\n";
		double lk0 = conditional_llk(v);
		double h1 = g[v].height+add;
		g[v].height = h1;
		set_ls(v);
		double lk1 = conditional_llk(v);
		double h2 = g[v].height + add;
		g[v].height = h2;
		set_ls(v);
		double lk2 = conditional_llk(v);
		//cout << h0 << " "<< h1 << " "<< h2 << " "<< lk0 << " "<< lk1 << " "<< lk2 << "\n";
		bool stop = false;
		while (!stop){
			//cout << "here2\n";
			double iterate = quad_opt_h(h0, h1, h2, lk0, lk1, lk2, g[v].height+1, m+params->b_min);
			g[v].height = iterate;
			set_ls(v);

			double newlk = conditional_llk(v);
			//cout << newlk << " "<< lk2 << " "<< iterate << " "<< h2 << "\n";
			if ( fabs(newlk - lk2) < params->e_lk  ||  fabs(iterate - h2) < params->e_step) stop = true;
			else{
				h0 = h1;
				h1 = h2;
				h2 = iterate;
				lk0 = lk1;
				lk1 = lk2;
				lk2 = newlk;
			}
		}

	}
}

void PopTree::optimize_heights(vector<Graph::vertex_descriptor> inv){
	vector<double> cond;
	for (vector<Graph::vertex_descriptor>::iterator it = inv.begin(); it != inv.end(); it++)	{
		//cout <<"conditional "<< " "<< g[*it].index << " "<< conditional_llk(*it)<< "\n";
		cond.push_back( conditional_llk(*it));
	}
	bool done = false;
	while (!done){
		int updated = 0;
		for (int i = 0; i < inv.size(); i++){
			optimize_height_gs(inv[i]);
			double newlk = conditional_llk(inv[i]);
			//cout << newlk << " "<< g[inv[i]].index << "\n";
			if (newlk > cond[i]+params->e_lk){
				cond[i] = newlk;
				updated ++;
			}
		}
		if (updated < 1) done = true;
	}
}

vector<Graph::vertex_descriptor> PopTree::get_path_to_root_vec(Graph::vertex_descriptor v){
	vector<Graph::vertex_descriptor> toreturn;
	while (!g[v].is_root){
		toreturn.push_back(v);
		v = get_parent_node(v).first;
	}
	toreturn.push_back(root);
	return toreturn;
}
void PopTree::optimize_height_gs(Graph::vertex_descriptor v){
	if (!g[v].is_root & !g[v].is_tip){
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(v);
		double m = g[ch.first].height;
		double m2 =g[ch.second].height;
		if (m2 > m) m = m2;
		Graph::vertex_descriptor p = get_parent_node(v).first;
		double mi = m;
		double ma = g[p].height;
		double guess = g[v].height;
		if ( (ma-mi) < 2*params->b_min){
			g[v].height = (mi+ma)/2;
			set_ls(v);
			set_ls(p);
		}
		else{
			int nit = 0;
		//cout << m+params->b_min << " "<< guess << " "<< ma-params->b_min<< "\n";
			golden_section_h(v, p, mi+params->b_min, guess, ma- params->b_min, params->tau, &nit);
		}

	}
	else if (g[v].is_root){
		//cout << "here1\n";
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(v);
		double m = g[ch.first].height;
		double m2 =g[ch.second].height;
		if (m2 > m) m = m2;
		double guess = g[v].height;
		int nit =0;
		golden_section_h(v, v, m+params->b_min, guess, g[v].height+1, params->tau, &nit);
	}
}

double PopTree::quad_opt_h(double h0, double h1, double h2, double lk0, double lk1, double lk2, double max, double min){
	double n = (h0 - h1)*lk2 - (h0-h1)*lk0 +lk0*h0-  lk1*h0- h2*lk0 +lk1*h2;
	double d = h0*h0*h0 - h1*h1*h0 - (h0-h1)*h0*h0 - h2*h0*h0 +h1*h1*h2+h2*h2;
	double c = n/d;
	double b = ( lk0 - c*h0*h0 + c*h1*h1- lk1)/ (h0-h1);
	double a = lk0- b*h0- c*h0*h0;
	cout << a <<" "<< b << " "<< c << " "<< n << " "<< d << " "<< -b/(2*c)<< "\n";
	if ( c> 0) return min;
	else{
		double th = -b/ (2*c);
		if (th > max) return max;
		else if (th < min) return min;
		else return th;
	}
}

void PopTree::optimize_all_heights(){
	//cout << "here\n"; cout.flush();
	//cout << "ntip "<< ntip << "\n";
	vector<Graph::vertex_descriptor> post = get_postorder_traversal(ntip);
	//cout << "here2\n"; cout.flush();
	compute_tree_llk();
	double start_lk = current_llk;
	bool done = false;
	while (!done){
		for (vector<Graph::vertex_descriptor>::iterator it = post.begin(); it != post.end(); it++)	optimize_height_gs(*it);
		compute_tree_llk();
		double newlk = current_llk;
		//cout << newlk << " "<< start_lk << " lk\n";
		if (start_lk + params->e_lk < newlk)	start_lk = newlk;
		else done = true;
	}
	compute_tree_llk();
}

void PopTree::set_edge_lens(){
	pair<Graph::edge_iterator, Graph::edge_iterator> ed = edges(g);
	while (ed.first != ed.second){
		Graph::vertex_descriptor s = source(*ed.first, g);
		Graph::vertex_descriptor t = target(*ed.first, g);
		g[*ed.first].len = g[s].height - g[t].height;
		ed.first++;
	}

}
void PopTree::move_root(int i){
	/*
	 *  go from
	 *        root
	 *         / \
	 *        /   \
	 *       v1    v2
	 *      / \    / \
	 *     /   \  v5  v6
	 *    v3   v4
	 *

	 *
	 */

	Graph::vertex_descriptor v1, v2, v3, v4, v5, v6;
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children = get_child_nodes(root);
	Graph::edge_descriptor er1, er2;
	double h1, h2, h3, h4, h5, h6;
	double lr1, lr2;
	v1 = children.first;
	v2 = children.second;
	h1 = g[v1].height;
	h2 = g[v2].height;
	if ( !g[v1].is_tip ){
		children = get_child_nodes(v1);
		v3 = children.first;
		v4 = children.second;
		h3 = g[v3].height;
		h4 = g[v4].height;
	}
	if ( !g[v2].is_tip ){
		children = get_child_nodes(v2);
		v5 = children.first;
		v6 = children.second;
		h5 = g[v5].height;
		h6 = g[v6].height;
	}
	er1 = edge(root, v1, g).first;
	er2 = edge(root, v2, g).first;
	lr1 = g[er1].len;
	lr2 = g[er2].len;

	 /*      to
		 *
		 *   i == 1
		 *
		 *    root
		 *     / \
		 *    /   \
		 *   v3    v1
		 *        / \
		 *       /   \
		 *      v4    v2
*/

	if ( i == 1 && !g[v1].is_tip){
		double l13;
		Graph::edge_descriptor e13 = edge(v1, v3, g).first;
		l13 = g[e13].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e13, g);
		Graph::edge_descriptor e = add_edge(v1, v2, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v3, g).first;
		g[e].len = l13/2;

		g[e].weight = 1;

		e = add_edge(root, v1, g).first;
		g[e].len = l13/2;

		g[e].weight = 1;
		if (h4> h2) g[v1].height = h4+0.01;
		else g[v1].height = h2+0.01;

		if (h3 > g[v1].height) g[root].height = h3+0.01;
		else g[root].height = g[v1].height+ 0.01;

		vector<Graph::vertex_descriptor> opt;
		opt.push_back(v3); opt.push_back(v1);opt.push_back(root);
		optimize_heights(opt);
	}

	 /*
	 *   i == 2
	 *
	 *    root
	 *     / \
	 *    /   \
	 *   v4    v1
	 *        / \
	 *       /   \
	 *      v3    v2
	 */
	else if ( i == 2 && !g[v1].is_tip){
		double l14;
		Graph::edge_descriptor e14 = edge(v1, v4, g).first;
		l14 = g[e14].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e14, g);
		Graph::edge_descriptor e = add_edge(v1, v2, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v4, g).first;
		g[e].len = l14/2;

		g[e].weight = 1;

		e = add_edge(root, v1, g).first;
		g[e].len = l14/2;

		g[e].weight = 1;
		if (h3> h2) g[v1].height = h3+0.01;
		else g[v1].height = h2+0.01;

		if (h4 > g[v1].height) g[root].height = h4+0.01;
		else g[root].height = g[v1].height+ 0.01;

		vector<Graph::vertex_descriptor> opt;
		opt.push_back(v4); opt.push_back(v1);opt.push_back(root);
		optimize_heights(opt);
	}

	/*   i == 3
	 *
	 *    root
	 *     / \
	 *    /   \
	 *   v5    v2
	 *        / \
	 *       /   \
	 *      v1    v6
	 */
	else if ( i == 3 && !g[v2].is_tip){
		double l25;
		Graph::edge_descriptor e25 = edge(v2, v5, g).first;
		l25 = g[e25].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e25, g);
		Graph::edge_descriptor e = add_edge(v2, v1, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v5, g).first;
		g[e].len = l25/2;

		g[e].weight = 1;

		e = add_edge(root, v2, g).first;
		g[e].len = l25/2;

		g[e].weight = 1;

		if (h1> h6) g[v2].height = h1+0.01;
		else g[v2].height = h6+0.01;

		if (h5 > g[v2].height) g[root].height = h5+0.01;
		else g[root].height = g[v2].height+ 0.01;

		vector<Graph::vertex_descriptor> opt;
		opt.push_back(v5); opt.push_back(v2);opt.push_back(root);
		optimize_heights(opt);
	}


	 /*   i == 4
	 *
	 *    root
	 *     / \
	 *    /   \
	 *   v6    v2
	 *        / \
	 *       /   \
	 *      v1    v5
	 */

	else if ( i == 4 && !g[v2].is_tip){
		double l26;
		Graph::edge_descriptor e26 = edge(v2, v6, g).first;
		l26 = g[e26].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e26, g);
		Graph::edge_descriptor e = add_edge(v2, v1, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v6, g).first;
		g[e].len = l26/2;

		g[e].weight = 1;

		e = add_edge(root, v2, g).first;
		g[e].len = l26/2;

		g[e].weight = 1;
		if (h1> h5) g[v2].height = h1+0.01;
		else g[v2].height = h5+0.01;

		if (h6 > g[v2].height) g[root].height = h6+0.01;
		else g[root].height = g[v2].height+ 0.01;

		vector<Graph::vertex_descriptor> opt;
		opt.push_back(v6); opt.push_back(v2);opt.push_back(root);
		optimize_heights(opt);

	}
}

/*

Graph::vertex_descriptor PopTree::add_tip(Graph::vertex_descriptor v, string name){
	// add a tip attached above v, halfway to the parent node
	Graph::vertex_descriptor vtipnew, vintnew, vparent;
	double len, newlen;
	Graph::edge_descriptor e;

	if (g[v].is_root == false){
		// remove edge from parent to target
		graph_traits<Graph>::in_edge_iterator init = in_edges(v, g).first;
		len = g[*init].len;
		newlen = len/2;
		vparent = source(*init, g);
		remove_edge(*init, g);

		//add new vertices
		vtipnew = add_vertex(g);
		vintnew = add_vertex(g);

		g[vtipnew].index = indexcounter;
		g[vtipnew].name = name;
		g[vtipnew].height = 0;
		g[vtipnew].is_tip = true;
		g[vtipnew].is_root = false;
		g[vtipnew].rev = false;
		g[vtipnew].pop = 0;
		indexcounter++;

		g[vintnew].index = indexcounter;
		g[vintnew].name = "NA";
		g[vintnew].height = 0;
		g[vintnew].is_tip = false;
		g[vintnew].is_root = false;
		g[vintnew].rev = false;
		g[vintnew].pop = 0;
		indexcounter++;

		// add new edges
		e = add_edge( vparent, vintnew, g).first;
		g[e].weight = 1;
		g[e].len = newlen;

		e = add_edge( vintnew, v, g).first;
		g[e].weight = 1;
		g[e].len = newlen;

		e = add_edge( vintnew, vtipnew, g).first;
		g[e].weight = 1;
		g[e].len = 1;

		return vtipnew;
	}
	else{

		//add a new root
		g[v].is_root = false;
		vintnew = add_vertex(g);
		vtipnew = add_vertex(g);
		root = vintnew;
		g[vtipnew].index = indexcounter;
		g[vtipnew].name = name;
		g[vtipnew].height = 0;
		g[vtipnew].is_tip = true;
		g[vtipnew].is_root = false;
		g[vtipnew].rev = false;
		g[vtipnew].pop  = 0;
		indexcounter++;

		g[vintnew].index = indexcounter;
		g[vintnew].name = "NA";
		g[vintnew].height = 0;
		g[vintnew].is_tip = false;
		g[vintnew].is_root = true;
		g[vintnew].rev = false;
		g[vtipnew].pop = 0;
		indexcounter++;

		// add new edges
		e = add_edge( vintnew, v, g).first;
		g[e].weight = 1;
		g[e].len = 1;
		e = add_edge( vintnew, vtipnew, g).first;
		g[e].weight = 1;
		g[e].len = 1;

		return vtipnew;
	}
}
*/

/*
void PopTree::remove_tip(Graph::vertex_descriptor v){
	Graph::vertex_descriptor vparent, vparent2, vnewdest;
	double newlen;
	Graph::edge_descriptor e;


	graph_traits<Graph>::in_edge_iterator init = in_edges(v, g).first;
	vparent = source(*init, g);

	//if the node isn't coming from the root
	if (g[vparent].is_root == false){

		/*
		 *     vp2
		 *     / \
		 *    /   \
		 *         vp
		 *         / \
		 *        /   \
		 *       v     vnewdest
		 *
		 *
		 *  delete v, vp, draw edge from vp2 to vnewdest
		 */
/*		graph_traits<Graph>::in_edge_iterator init2 = in_edges(vparent, g).first;
		vparent2 = source(*init2, g);

		remove_edge(*init, g);

		graph_traits<Graph>::out_edge_iterator outit = out_edges(vparent, g).first;
		vnewdest = target(*outit, g);
		newlen = g[*outit].len+ g[*init2].len;

		e = add_edge(vparent2, vnewdest, g).first;
		g[e].len = newlen;
		g[e].weight = 1;

		clear_vertex(vparent, g);
		remove_vertex(vparent, g);
		remove_vertex(v, g);
	}

	//otw

	else{

		/*
		 *
		 *         vp
		 *        /  \
		 *       /    \
		 *      v      vnewdest
		 *
		 * delete v, vp, make vnewdest the new root
		 *
		 */
/*		remove_edge(*init, g);
		graph_traits<Graph>::out_edge_iterator outit = out_edges(vparent, g).first;
		vnewdest = target(*outit, g);
		g[vnewdest].is_root = true; //this is the new root
		root = vnewdest;
		clear_vertex(vparent, g);
		remove_vertex(vparent, g);
		remove_vertex(v, g);
 	}
}


void PopTree::copy(PopTree * s){
	popnames = s->popnames;


	//cout << "copying graph\n"; cout.flush();
	g.clear();
	map<int, Graph::vertex_descriptor> tmpmap;
	Graph::vertex_descriptor vd;
	pair<Graph::vertex_iterator, Graph::vertex_iterator> v = vertices(s->g);
	pair<Graph::edge_iterator, Graph::edge_iterator> e = edges(s->g);
	for (Graph::vertex_iterator it = v.first; it != v.second; ++it){
		vd = add_vertex(g);
		g[vd].index = s->g[*it].index;
		g[vd].name = s->g[*it].name;
		g[vd].height = s->g[*it].height;
		g[vd].rev = s->g[*it].rev;
		g[vd].is_root = s->g[*it].is_root;
		g[vd].is_tip = s->g[*it].is_tip;
		g[vd].pop = s->g[*it].pop;
		tmpmap.insert(make_pair( g[vd].index, vd));
		if (s->g[*it].is_root == true) root = vd;
	}

	for (Graph::edge_iterator it = e.first; it != e.second; ++it){
		Graph::vertex_descriptor tmpsource = source(*it, s->g);
		Graph::vertex_descriptor tmptarget = target(*it, s->g);
		Graph::edge_descriptor ed = add_edge( tmpmap[ s->g[tmpsource].index ], tmpmap[ s->g[tmptarget].index ], g ).first;
		g[ed].weight = s->g[*it].weight;
		g[ed].len = s->g[*it].len;
	}
}

PopTree::PopTree(string p_newickString){
	g = Graph(1);
	index2father.clear();
	int i = 0;
	Graph::vertex_descriptor v = *vertices(g).first;
	Graph::vertex_descriptor v2 = *vertices(g).first;
	Graph::edge_descriptor e;
	g[v].index = i;
	g[v].name = "NA";
	g[v].height = 0;
	index2father.push_back(NULL);
	g[v].is_tip = false;
	g[v].is_root = true;
	g[v].rev = false;
	g[v].pop = 0;
	root = v;
	for(string::const_iterator I = p_newickString.begin();
		I != p_newickString.end(); ++I)
      {
		//cout << *I << "\n";
		if ( *I == '(' )//Begin new subtree
		{
			v2 = add_vertex(g);
			i++;
			g[v2].index = i;
			g[v2].name = "NA";
			g[v2].height = 0;
			index2father.push_back(v);
			g[v2].is_tip = false;
			g[v2].is_root = false;
			g[v2].rev = false;
			g[v2].pop = 0;
			e = add_edge( v, v2, g).first;
			v = v2;
		}
		else if( *I == ')' )// Subtree finished, get back
		{

			v = index2father[g[v].index];
			//e = add_edge( index2father[g[v].index], v, g).first;

		}
		else if( *I == ',' )// insert brother
		{
			v = index2father[g[v].index];
			v2 = add_vertex(g);
			i++;
			g[v2].index = i;
			g[v2].name = "NA";
			g[v2].height = 0;
			index2father.push_back(v);
			g[v2].is_tip = false;
			g[v2].is_root = false;
			g[v2].rev = false;
			g[v2].pop = 0;
			e = add_edge( v, v2, g).first;
			v = v2;
      }
      else if( *I == ':' )// treelength
      {
              std::string length = "";
              ++I;
              while( *I!=',' && *I!=')' && *I!=':' && *I!='(' && *I!=';')
              {
                      length += *I;
                      ++I;
              }
              --I;
              graph_traits<Graph>::in_edge_iterator in_i;
              graph_traits<Graph>::in_edge_iterator in_end;
              for ( tie(in_i, in_end)= in_edges(v, g); in_i != in_end; ++in_i ){
            	  g[*in_i].len = atof(length.c_str());
            	  g[*in_i].weight = 1;
              }
      }
      else if( *I == ';' )
      {
              break;
      }
      else// name
      {
              std::string name = "";
              do
              {
                      name += *I;
                      ++I;
              }
              while( *I!=',' && *I!=')' && *I!=':' && *I!='(' && *I!=';');
              --I;
              g[v2].name = name;
              g[v2].is_tip = true;
              popnames.push_back(name);
      }
      }
}



void PopTree::set_graph(string p_newickString){
	g.clear();
	add_vertex(g);
	index2father.clear();
	popnames.clear();
	int i = 0;
	Graph::vertex_descriptor v = *vertices(g).first;
	Graph::vertex_descriptor v2 = *vertices(g).first;
	Graph::edge_descriptor e;
	g[v].index = i;
	g[v].name = "NA";
	g[v].height = 0;
	index2father.push_back(NULL);
	g[v].is_tip = false;
	g[v].is_root = true;
	g[v].rev = false;
	g[v].pop = 0;
	root = v;
	for(string::const_iterator I = p_newickString.begin();
		I != p_newickString.end(); ++I)
      {
		//cout << *I << "\n";
		if ( *I == '(' )//Begin new subtree
		{
			v2 = add_vertex(g);
			i++;
			g[v2].index = i;
			g[v2].name = "NA";
			g[v2].height = 0;
			index2father.push_back(v);
			g[v2].is_tip = false;
			g[v2].is_root = false;
			g[v2].rev = false;
			g[v2].pop = 0;
			e = add_edge( v, v2, g).first;
			v = v2;
		}
		else if( *I == ')' )// Subtree finished, get back
		{
			v = index2father[g[v].index];
		}
		else if( *I == ',' )// insert brother
		{
			v = index2father[g[v].index];
			v2 = add_vertex(g);
			i++;
			g[v2].index = i;
			g[v2].name = "NA";
			g[v2].height = 0;
			index2father.push_back(v);
			g[v2].is_tip = false;
			g[v2].is_root = false;
			g[v2].rev = false;
			g[v2].pop = 0;
			e = add_edge( v, v2, g).first;
			v = v2;
      }
      else if( *I == ':' )// treelength
      {
              std::string length = "";
              ++I;
              while( *I!=',' && *I!=')' && *I!=':' && *I!='(' && *I!=';')
              {
                      length += *I;
                      ++I;
              }
              --I;
              graph_traits<Graph>::in_edge_iterator in_i;
              graph_traits<Graph>::in_edge_iterator in_end;
              for ( tie(in_i, in_end)= in_edges(v, g); in_i != in_end; ++in_i ){
            	  g[*in_i].len = atof(length.c_str());
            	  g[*in_i].weight = 1;
              }
      }
      else if( *I == ';' )
      {
              break;
      }
      else// name
      {
              std::string name = "";
              do
              {
                      name += *I;
                      ++I;
              }
              while( *I!=',' && *I!=')' && *I!=':' && *I!='(' && *I!=';');
              --I;
              g[v2].name = name;
              g[v2].is_tip = true;
              popnames.push_back(name);
      }
      }
	indexcounter = i+1;
}


set<Graph::vertex_descriptor> PopTree::get_root_adj(){
	set<Graph::vertex_descriptor> toreturn;
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> adj = get_child_nodes(root);
	toreturn.insert( adj.first);
	toreturn.insert( adj.second);
	return toreturn;
}


set<Graph::edge_descriptor> PopTree::get_root_adj_edge(){
	set<Graph::edge_descriptor> toreturn;
	pair<Graph::out_edge_iterator, Graph::out_edge_iterator> oute = out_edges(root, g);
	toreturn.insert( *oute.first);
	oute.first++;
	toreturn.insert( *oute.first);
	return toreturn;
}



void PopTree::flip_sons(Graph::vertex_descriptor v, gsl_rng* r){
	if (g[v].is_tip == false){
		double ran = gsl_rng_uniform(r);
		if (ran <0.5){
			if (g[v].rev == true) g[v].rev = false;
			else if (g[v].rev == false) g[v].rev = true;
		}
		graph_traits<Graph>::out_edge_iterator out_i = out_edges(v, g).first;
		flip_sons(target(*out_i, g), r);
		++out_i;
		flip_sons(target(*out_i, g), r);
	}
}

vector<Graph::vertex_descriptor > PopTree::get_inorder_traversal(int nodes){
	vector<Graph::vertex_descriptor> toreturn(2*nodes-1);
	int count = 0;
	inorder_traverse(root, &count, &toreturn);
	return toreturn;
}


vector<Graph::vertex_descriptor > PopTree::get_inorder_traversal_fromindex(int index){
	map<int, Graph::vertex_descriptor> i2v = index2vertex();
	map<string, Graph::vertex_descriptor> t = get_tips( i2v[index]);
	int nodes = t.size();
	vector<Graph::vertex_descriptor> toreturn1(2*nodes-1);
	vector<Graph::vertex_descriptor> toreturn;
	int count = 0;
	inorder_traverse(i2v[index], &count, &toreturn1);
	for (vector<Graph::vertex_descriptor>::iterator it = toreturn1.begin(); it < toreturn1.end(); it++) if (g[*it].index != index) toreturn.push_back(*it);
	return toreturn;
}



vector<Graph::vertex_descriptor > PopTree::get_inorder_traversal_noroot(int nodes){
	vector<Graph::vertex_descriptor> toreturn;
	vector<Graph::vertex_descriptor> tmp = get_inorder_traversal(nodes);
	for (int i = 0; i < tmp.size(); i++){
		if (g[tmp[i]].is_root == false) toreturn.push_back(tmp[i]);
	}
	return toreturn;
}
void PopTree::inorder_traverse(Graph::vertex_descriptor rootIterator, int* i, vector<Graph::vertex_descriptor >* v){
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> child_nodes = get_child_nodes(rootIterator);
	//graph_traits<Graph>::out_edge_iterator out_i = out_edges(rootIterator, g).first;
	//cout << g[rootIterator].index << " "<< g[rootIterator].name << " "<< g[rootIterator].is_tip << " "<< g[rootIterator].rev << "\n";
	if (g[rootIterator].is_tip == false){
		//inorder_traverse(target(*out_i, g), i , v);
		inorder_traverse(child_nodes.first, i , v);
 	}

 	v->at(*i) = rootIterator;
 	*i = *i+1;

 	if (g[rootIterator].is_tip == false){
 		inorder_traverse(child_nodes.second, i, v);
 	}
}

Graph::vertex_descriptor PopTree::get_LCA(Graph::vertex_descriptor root_v,
		Graph::vertex_descriptor tip1_v, Graph::vertex_descriptor tip2_v){
		//cout << "in get_LCA "<< g[root_v].index << " "<< g[tip1_v].index << " "<< g[tip2_v].index << "\n"; cout.flush();
		if (g[root_v].is_tip == true) return NULL;
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children = get_child_nodes(root_v);
		Graph::vertex_descriptor c1 = children.first;
		Graph::vertex_descriptor c2 = children.second;
		graph_traits<Graph>::out_edge_iterator out_i = out_edges(root_v, g).first;
		bool found = false;
       	if (g[c1].index == g[tip1_v].index || g[c1].index == g[tip2_v].index) found = true;
       	if (g[c2].index == g[tip1_v].index || g[c2].index == g[tip2_v].index) found = true;
       	if (found)  return root_v;
       	else{
       		Graph::vertex_descriptor firstit = get_LCA(c2, tip1_v, tip2_v);
       		Graph::vertex_descriptor lastit = get_LCA(c1, tip1_v, tip2_v);
       		if (firstit && lastit) return root_v;
       		else if (firstit) return firstit;
       		else return lastit;
       	}
}



map<string, Graph::vertex_descriptor> PopTree::get_tips(Graph::vertex_descriptor p_rootIterator){
	map<string, Graph::vertex_descriptor> toreturn;
  	if (out_degree(p_rootIterator, g) ==0){
 		toreturn.insert(make_pair(g[p_rootIterator].name, p_rootIterator));
 	}
 	else{
 		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children =  get_child_nodes(p_rootIterator);
 		map<string, Graph::vertex_descriptor> t1 = get_tips(children.first);


 		map<string, Graph::vertex_descriptor> t2 = get_tips(children.second);
 		for (map<string, Graph::vertex_descriptor >::iterator it1 = t1.begin(); it1 != t1.end(); it1++) toreturn.insert(make_pair(it1->first, it1->second));
 		for (map<string, Graph::vertex_descriptor>::iterator it2 = t2.begin(); it2 != t2.end(); it2++)	toreturn.insert(make_pair(it2->first, it2->second));

 	}
 	return toreturn;
}

void PopTree::move_root(gsl_rng* r){
	double ran = gsl_rng_uniform(r);
	Graph::out_edge_iterator it = out_edges(root, g).first;
	Graph::vertex_descriptor target;
	if (ran < 0.5) it++;

	//target = target(*it, g);


}


void PopTree::local_rearrange(Graph::vertex_descriptor v3, int i){
	/*
	 *
	 * go from    1
	 *          /  \*
	 *         /    3
	 *        /    / \
	 *       2    4   5
	 *
	 *  if i == 1, go to
	 *
	 *           1
	 *          / \
	 *         3   \
	 *        / \   \
	 *       2   4   5
	 *
	 *  if i == 2, go to
	 *
	 *           1
	 *          / \
	 *         3   \
	 *        / \   \
	 *       2   5   4
	 *
	 *
	 */
/*	if (!g[v3].is_root){

		// set up descriptors and edge lengths
		Graph::in_edge_iterator tmp = in_edges(v3, g).first;
		Graph::edge_descriptor ed = *tmp;
		Graph::vertex_descriptor v1, v2, v4, v5;
		double d12, d13, d34, d35;
		double l2, l4, l5;
		d13 = g[ed].len;
		v1 = source(ed, g);
		Graph::out_edge_iterator out3 = out_edges(v3, g).first;
		v4 = target(*out3, g);
		d34 = g[*out3].len;

		++out3;
		v5 = target(*out3, g);
		d35 = g[*out3].len;

		Graph::out_edge_iterator out1 = out_edges(v1, g).first;
		if (target(*out1, g) == v3) {
			out1++;
			v2 = target(*out1, g);
			d12 = g[*out1].len;
		}
		else {
			v2 = target(*out1, g);
			d12 = g[*out1].len;
		}
		l2 = d12;
		l4 = d13+d34;
		l5 = d13+d35;
		double min = l2;
		if (l4 < min) min = l4;
		if (l5 < min) min = l5;

		if (i == 1){
		// go to ((2,4),5)

			remove_edge(v1, v2, g);
			remove_edge(v3, v5, g);
			Graph::edge_descriptor e = add_edge(v3, v2, g).first;
			g[e].weight = 1;
			g[e].len = d12;

			e = add_edge(v1, v5, g).first;
			g[e].weight = 1;
			g[e].len = d35;


		}
		else{
			//go to (4,(2,5))

			remove_edge(v1, v2, g);
			remove_edge(v3, v4, g);
			Graph::edge_descriptor e = add_edge(v3, v2, g).first;
			g[e].weight = 1;
			g[e].len = d12;


			e = add_edge(v1, v4, g).first;
			g[e].weight = 1;
			g[e].len = d34;

		}
	}

}

void PopTree::move_root(int i){
	/*
	 *  go from
	 *        root
	 *         / \
	 *        /   \
	 *       v1    v2
	 *      / \    / \
	 *     /   \  v5  v6
	 *    v3   v4
	 *

	 *
	 */

/*	Graph::vertex_descriptor v1, v2, v3, v4, v5, v6;
	pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children = get_child_nodes(root);
	Graph::edge_descriptor er1, er2;
	double lr1, lr2;
	v1 = children.first;
	v2 = children.second;
	if ( !g[v1].is_tip ){
		children = get_child_nodes(v1);
		v3 = children.first;
		v4 = children.second;
	}
	if ( !g[v2].is_tip ){
		children = get_child_nodes(v2);
		v5 = children.first;
		v6 = children.second;
	}
	er1 = edge(root, v1, g).first;
	er2 = edge(root, v2, g).first;
	lr1 = g[er1].len;
	lr2 = g[er2].len;

	 /*      to
		 *
		 *   i == 1
		 *
		 *    root
		 *     / \
		 *    /   \
		 *   v3    v1
		 *        / \
		 *       /   \
		 *      v4    v2
*/
/*
	if ( i == 1 && !g[v1].is_tip){
		double l13;
		Graph::edge_descriptor e13 = edge(v1, v3, g).first;
		l13 = g[e13].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e13, g);
		Graph::edge_descriptor e = add_edge(v1, v2, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v3, g).first;
		g[e].len = l13/2;

		g[e].weight = 1;

		e = add_edge(root, v1, g).first;
		g[e].len = l13/2;

		g[e].weight = 1;
	}

	 /*
	 *   i == 2
	 *
	 *    root
	 *     / \
	 *    /   \
	 *   v4    v1
	 *        / \
	 *       /   \
	 *      v3    v2
	 */
/*	else if ( i == 2 && !g[v1].is_tip){
		double l14;
		Graph::edge_descriptor e14 = edge(v1, v4, g).first;
		l14 = g[e14].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e14, g);
		Graph::edge_descriptor e = add_edge(v1, v2, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v4, g).first;
		g[e].len = l14/2;

		g[e].weight = 1;

		e = add_edge(root, v1, g).first;
		g[e].len = l14/2;

		g[e].weight = 1;

	}

	/*   i == 3
	 *
	 *    root
	 *     / \
	 *    /   \
	 *   v5    v2
	 *        / \
	 *       /   \
	 *      v1    v6
	 */
/*	else if ( i == 3 && !g[v2].is_tip){
		double l25;
		Graph::edge_descriptor e25 = edge(v2, v5, g).first;
		l25 = g[e25].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e25, g);
		Graph::edge_descriptor e = add_edge(v2, v1, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v5, g).first;
		g[e].len = l25/2;

		g[e].weight = 1;

		e = add_edge(root, v2, g).first;
		g[e].len = l25/2;

		g[e].weight = 1;

	}


	 /*   i == 4
	 *
	 *    root
	 *     / \
	 *    /   \
	 *   v6    v2
	 *        / \
	 *       /   \
	 *      v1    v5
	 */

/*	else if ( i == 4 && !g[v2].is_tip){
		double l26;
		Graph::edge_descriptor e26 = edge(v2, v6, g).first;
		l26 = g[e26].len;
		remove_edge( er1, g);
		remove_edge( er2, g);
		remove_edge( e26, g);
		Graph::edge_descriptor e = add_edge(v2, v1, g).first;
		g[e].len = lr1+lr2;

		g[e].weight = 1;

		e = add_edge(root, v6, g).first;
		g[e].len = l26/2;

		g[e].weight = 1;

		e = add_edge(root, v2, g).first;
		g[e].len = l26/2;

		g[e].weight = 1;

	}
}

void PopTree::global_rearrange(Graph::vertex_descriptor v1, Graph::vertex_descriptor v2){
	/*
	 *   take the tree below v1p, attach it above v2
	 *   if v2 is the root, there's a special case
	 *   if v1p is the root there's another special case
	 *   do not do anything if v1 is the root
	 *
	 *   otherwise:
	 *
	 *             v1pp
	 *             /  \
	 *            /    \
	 *          v1p                  v2p
	 *          / \                  / \
	 *         /   \                /   \
	 *        v1   v1s            v2
	 *       / \
	 *      /   \
	 *
	 *      goes to
	 *
	 *
	 *      v1pp                     v2p
	 *      / \                      /  \
	 *     /   \                    /    \
	 *    v1s                      v1p
	 *                            / \
	 *                           /   \
	 *                          v2   v1
	 */
/*	 Graph::vertex_descriptor v1p, v1pp, v1s, v2p;
	 Graph::edge_descriptor e;
	 if (!g[v1].is_root && !g[v2].is_root && !g[get_parent_node(v1).first].is_root){
		 v1p = get_parent_node(v1).first;
		 v1pp = get_parent_node(v1p).first;
		 v2p = get_parent_node(v2).first;
		 pair<Graph::vertex_descriptor, Graph::vertex_descriptor> d_v1p = get_child_nodes(v1p);
		 if (d_v1p.first == v1) v1s = d_v1p.second;
		 else v1s = d_v1p.first;

		 // take care of the tree with v1
		 remove_edge(v1pp, v1p, g);
		 remove_edge(v1p, v1s, g);
		 e = add_edge(v1pp, v1s, g).first;
		 g[e].weight = 1;
		 g[e].len = 1;


		 //and the tree with v2
		 remove_edge(v2p, v2, g);
		 e = add_edge(v2p, v1p, g).first;
		 g[e].weight = 1;
		 g[e].len = 1;

		 e = add_edge(v1p, v2, g).first;
		 g[e].weight = 1;
		 g[e].len = 1;

	 }
	 else if (g[v2].is_root){
		 /*
		  *      go to
		  *
		  *
		  *      v1pp
		  *      / \
		  *     /   \
		  *    v1s                      v1p
		  *                            / \
		  *                           /   \
		  *                          v2   v1
		  *
		  *
		  *
		  */
/*		 v1p = get_parent_node(v1).first;
		 v1pp = get_parent_node(v1p).first;
		 pair<Graph::vertex_descriptor, Graph::vertex_descriptor> d_v1p = get_child_nodes(v1p);
		 if (d_v1p.first == v1) v1s = d_v1p.second;
		 else v1s = d_v1p.first;

		 // take care of the tree with v1
		 remove_edge(v1pp, v1p, g);
		 remove_edge(v1p, v1s, g);
		 e = add_edge(v1pp, v1s, g).first;
		 g[e].weight = 1;
		 g[e].len = 1;


		 //and the tree with v2
		 e = add_edge(v1p, v2, g).first;
		 g[e].weight = 1;
		 g[e].len = 1;

		 set_root(v1p);
	 }

	 /*
	  *    v1p
	  *    / \
	  *   /   \           v2p
	  *  v1    v1s        / \
	  *        / \       /   \
	  *       /   \      v2
	  *
	  *
	  *  to
	  *
	  *     v1s (new root)     v2p
	  *     / \                /  \
	  *    /   \              v1p
	  *                       /  \
	  *                      v2   v1
	  *
	  */
/*	 else if (!g[v1].is_root && !g[v2].is_root && g[get_parent_node(v1).first].is_root){
		 //cout << "HERE\n";
		 //print();
		 //cout << "\n";
		 Graph::vertex_descriptor v1s;
		 v1p = get_parent_node(v1).first;
		 v2p = get_parent_node(v2).first;
		 pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(v1p);
		 if (ch.first == v1 ) v1s = ch.second;
		 else v1s = ch.first;

		 // tree 1
		 remove_edge(v1p, v1s, g);
		 set_root(v1s);

		 // tree 2
		 e = edge(v2p, v2, g).first;
		 double l = g[e].len;
		 remove_edge(e, g);
		 e = add_edge(v2p, v1p, g).first;
		 g[e].len = l/2;

		 g[e].weight = 1;

		 e = add_edge(v1p, v2, g).first;
		 g[e].len = l/2;

		 g[e].weight = 1;
	 }



}

void PopTree::set_root(Graph::vertex_descriptor v){
	g[root].is_root = false;
	g[v].is_root = true;
	root = v;
}

string PopTree::get_newick_format(){
 	string toreturn = "";
 	newick_helper(root, &toreturn);
 	return toreturn;
 }


string PopTree::get_newick_format(Graph::vertex_descriptor v){
 	string toreturn = "";
 	newick_helper(v, &toreturn);
 	return toreturn;
 }

void PopTree::print(string stem){
	string outvfile = stem+".vertices.gz";
	string outefile = stem+".edges.gz";
	ogzstream outv(outvfile.c_str());
	ogzstream oute(outefile.c_str());
	IndexMap index = get(&Node::index, g);
	pair<vertex_iter, vertex_iter> vp;
	for (vp = vertices(g); vp.first != vp.second; ++vp.first){
		outv << index[*vp.first] <<  " "<< g[*vp.first].name << " ";
		if (g[*vp.first].is_root) outv << "ROOT ";
		else outv << "NOT_ROOT ";
		if (g[*vp.first].is_tip) outv << "TIP ";
		else outv << "NOT_TIP ";


		outv << g[get_parent_node(*vp.first).first].index << " ";
		if (g[*vp.first].is_tip) outv << "NA NA NA NA ";
		else {
			pair<Graph::vertex_descriptor, Graph::vertex_descriptor> ch = get_child_nodes(*vp.first);
			outv << g[ch.first].index << " "<< get_tips(ch.first).size() << " "<< g[ch.second].index << " "<< get_tips(ch.second).size()<< " ";
		}
		outv << get_newick_format(*vp.first) <<"\n";
	}

    graph_traits<Graph>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei){
        oute << index[source(*ei, g)] << " "<< index[target(*ei, g)] << " "<< g[*ei].len << "\n";
    }
}




void PopTree::newick_helper(Graph::vertex_descriptor node, string* s){
	if (out_degree(node, g) !=  0){
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children = get_child_nodes(node);
		s->append("(");
		newick_helper(children.first, s);
		s->append(",");
		newick_helper(children.second, s);
		s->append(")");
		if (g[node].is_root == false){

			s->append(":");

			stringstream ss;
			ss<< get_parent_node(node).second;
			s->append(ss.str());
		}
		else s->append(";");
	}
	else{
		stringstream ss;
		ss << g[node].name;
		ss << ":";
		ss<< get_parent_node(node).second;
		s->append(ss.str());
	}

}


void PopTree::newick_helper(Graph::vertex_descriptor node, string* s, int rootindex){
	if (out_degree(node, g) !=  0){
		pair<Graph::vertex_descriptor, Graph::vertex_descriptor> children = get_child_nodes(node);
		s->append("(");
		newick_helper(children.first, s, rootindex);
		s->append(",");
		newick_helper(children.second, s, rootindex);
		s->append(")");
		if (g[node].index != rootindex){

			s->append(":");

			stringstream ss;
			ss<< get_parent_node(node).second;
			s->append(ss.str());
		}
		//else s->append(";");
	}
	else{
		stringstream ss;
		ss << g[node].name;
		if (g[node].index != rootindex) {
			ss << ":";
			ss<< get_parent_node(node).second;
		}
		s->append(ss.str());
	}

}

double PopTree::get_height(Graph::vertex_descriptor v){
	return g[v].height;
}


vector<Graph::vertex_descriptor> PopTree::get_path_to_root_vec(Graph::vertex_descriptor v){
	vector<Graph::vertex_descriptor> toreturn;
	while (g[v].is_root == false){
		toreturn.push_back(v);
		v = get_parent_node(v).first;
	}
	return toreturn;
}


set<Graph::vertex_descriptor> PopTree::get_path_to_root(Graph::vertex_descriptor v){
	set<Graph::vertex_descriptor> toreturn;
	while (g[v].is_root == false){
		toreturn.insert(v);
		v = get_parent_node(v).first;
	}
	return toreturn;
}

pair<Graph::vertex_descriptor, Graph::vertex_descriptor> PopTree::get_child_nodes(Graph::vertex_descriptor v){
	Graph::vertex_descriptor c1 = v;
	Graph::vertex_descriptor c2 = v;
	if (out_degree(v, g) != 2){
		return make_pair(c1, c2);
	}
	graph_traits<Graph>::out_edge_iterator out_i = out_edges(v, g).first;
	c1 = target(*out_i, g);
	out_i++;
	c2 = target(*out_i, g);
	return make_pair(c1, c2);
}

pair<Graph::vertex_descriptor, double> PopTree::get_parent_node(Graph::vertex_descriptor v){
	pair<Graph::vertex_descriptor, double> toreturn;
	if (g[v].is_root) {
		toreturn.first = v;
		toreturn.second = 0;
		return toreturn;
	}
	toreturn = get_parent_node(v);

	pair<Graph::vertex_descriptor, double> tmp = get_parent_node(toreturn.first);
	toreturn.first = tmp.first;
	toreturn.second += tmp.second;
	return toreturn;
}

map<int, Graph::vertex_descriptor> PopTree::index2vertex(){
	map<int, Graph::vertex_descriptor> toreturn;
	pair<Graph::vertex_iterator, Graph::vertex_iterator> v = vertices(g);
	while (v.first != v.second){
		toreturn.insert(make_pair( g[*v.first].index, *v.first ));
		v.first++;
	}
	return toreturn;
}

float PopTree::get_coaltime(Graph::vertex_descriptor v){
	float toreturn = 0;
	while (g[v].is_tip == false){
		Graph::out_edge_iterator e = out_edges(v, g).first;
		toreturn+= g[*e].len;
		//cout << g[*e].len << "l \n";
		v = target(*e, g);
	}
	//cout << toreturn << " return\n";
	return toreturn;
}
*/
